#include "map_receiver.h"
using namespace std;
using namespace cv;
void MapReceiver::OnMapReceived(const sensor_msgs::ImageConstPtr& msg) 
{
    cv_bridge::CvImagePtr cv_ptr; // 声明一个CvImage指针的实例

    try
    {
        cv_ptr =  cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC1); //TYPE_8UC1
    }
    catch(cv_bridge::Exception& e)  //异常处理
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    
    MapProcess(cv_ptr->image); // cv_ptr->image
}

// 地图处理程序
void MapReceiver::MapProcess(cv::Mat &img) 
{   
    cameraHeight = img.rows;    // 图像行数，高
    cameraWidth = img.cols;    	// 图像列数，宽
    static int firstFlag = true;
    if(firstFlag)
    {
        std::vector<unsigned char>().swap(cameraData);
        int size = cameraWidth*cameraHeight;
	cameraData.resize(size,255);
        //for(int i=0;i<size;i++)
        //    cameraData.push_back(255);
    }
    for (int i = 0; i < cameraHeight; ++i)
    {
        for (int j = 0; j < cameraWidth; ++j)
        {
            cameraData[(cameraHeight-1-i)*cameraWidth+(cameraWidth-1-j)] = img.at<uchar>(i, j);    // 获取第i行第j列数据, 0表示有障碍物，255表示无障碍物
        }
    }
    firstFlag = false;
    //cout << "Receive map: " << cols << " x " << rows << endl;
    //cv::imshow("Map", img);

    //cv::waitKey(5);
}
